#include "M0601C_driver.h"
#include <QDebug>

MotorDriver::MotorDriver(QObject *parent)
    : QObject(parent), serialPort(new QSerialPort())
{
    // 将驱动层移动到独立线程
    moveToThread(&workerThread);

    connect(&workerThread, &QThread::finished, this, &MotorDriver::deleteLater);
    connect(serialPort, &QSerialPort::readyRead, this, &MotorDriver::readResponse);

    workerThread.start();
}

MotorDriver::~MotorDriver()
{
    closeSerialPort();
    workerThread.quit();
    workerThread.wait();
    delete serialPort;
}

void MotorDriver::openSerialPort(const QString &portName, int baudRate)
{
    QMetaObject::invokeMethod(this, [=]() {
        if (serialPort->isOpen()) {
            emit errorOccurred("Serial port already open");
            return;
        }

        serialPort->setPortName(portName);
        serialPort->setBaudRate(baudRate);

        if (!serialPort->open(QIODevice::ReadWrite)) {
            emit errorOccurred("Failed to open serial port: " + serialPort->errorString());
        }
    });
}

void MotorDriver::closeSerialPort()
{
    QMetaObject::invokeMethod(this, [=]() {
        if (serialPort->isOpen()) {
            serialPort->close();
        }
    });
}

void MotorDriver::sendCommand(int motorId, const QByteArray &command)
{
    QMutexLocker locker(&mutex);
    commandQueue.enqueue(qMakePair(motorId, command));

    // 触发队列处理
    QMetaObject::invokeMethod(this, &MotorDriver::processQueue);
}

void MotorDriver::processQueue()
{
    if (commandQueue.isEmpty() || !serialPort->isOpen()) {
        return;
    }

    QMutexLocker locker(&mutex);
    QPair<int, QByteArray> currentCommand = commandQueue.dequeue();

    serialPort->write(currentCommand.second);
}


void MotorDriver::readResponse()
{
    QByteArray data = serialPort->readAll();

    // 解析响应，根据协议提取 motorId 和数据
    int motorId = UINT8_DATA(data[0]); 

    emit responseReceived(motorId, data);
}
